﻿///////////////////////////////////////////////////////////////////////////////
// Activity: program.activity
// UrbanClient service implementation
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;

using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Ccr.Core;
using W3C.Soap;

using ds = Microsoft.Dss.Services.Directory;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using urban = Microsoft.Robotics.Services.UrbanDashboard.Proxy;
using contactsensor = Microsoft.Robotics.Services.ContactSensor.Proxy;
using sicklrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using gps = Microsoft.Robotics.RoboChamps.Sensors.GPS.Proxy;


namespace Robotics.UrbanChallenge.UrbanClient
{
    [DisplayName("UrbanChallenge")]
    [Description("A user defined activity.")]
    [Contract(Contract.Identifier)]
    public class UrbanClientService : DsspServiceBase
    {
        //////////////////////////////////////////////
        /////////////////////////////////////////////
        /// REPLACE ME WITH YOUR OWN API KEY
        //////////////////////////////////////////////
        /////////////////////////////////////////////
        public const string API_KEY = "ABCDE12345";

        // Service state
        [InitialStatePartner(Optional = true)]
        private UrbanClientState _state;

        // Service operations port
        [ServicePort("/UrbanChallenge", AllowMultipleInstances = true)]
        private UrbanClientOperations _mainPort = new UrbanClientOperations();

        #region Partner services

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _subMgr = new submgr.SubscriptionManagerPort();

        [Partner("Bumpers", Contract = contactsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        contactsensor.ContactSensorArrayOperations _genericContactSensorsPort = new contactsensor.ContactSensorArrayOperations();
        contactsensor.ContactSensorArrayOperations _genericContactSensorsNotify = new contactsensor.ContactSensorArrayOperations();

        [Partner("LRF", Contract = sicklrf.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        sicklrf.SickLRFOperations _sickLaserRangeFinderPort = new sicklrf.SickLRFOperations();
        sicklrf.SickLRFOperations _sickLaserRangeFinderNotify = new sicklrf.SickLRFOperations();

        [Partner("DifferentialDrive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        drive.DriveOperations _genericDifferentialDrivePort = new drive.DriveOperations();

        [Partner("Dashboard", Contract = urban.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        urban.UrbanDashboardOperations _urbanDashboardPort = new urban.UrbanDashboardOperations();

        [Partner("WebCam1", Contract = webcam.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        webcam.WebCamOperations _webcam1Port = new webcam.WebCamOperations();
        webcam.WebCamOperations _webcam1Notify = new webcam.WebCamOperations();

        [Partner("WebCam2", Contract = webcam.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        webcam.WebCamOperations _webcam2Port = new webcam.WebCamOperations();
        webcam.WebCamOperations _webcam2Notify = new webcam.WebCamOperations();

        [Partner("GPS", Contract = gps.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
        gps.SimulatedGPSOperations _gpsPort = new gps.SimulatedGPSOperations();
        gps.SimulatedGPSOperations _gpsNotify = new gps.SimulatedGPSOperations();


        #endregion

        public UrbanClientService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        protected override void Start()
        {
            base.Start();

            SpawnIterator(DoStart);
        }

        private IEnumerator<ITask> DoStart()
        {
            // Send API Key
            yield return Arbiter.Choice(
                _urbanDashboardPort.SetAPIKey(new urban.SetAPIKeyRequest(API_KEY)),
                delegate(DefaultUpdateResponseType respone) { LogInfo("API KEY set"); },
                delegate(Fault f) { LogError(f); });
                      
            // Subscribe to partners
            yield return Arbiter.Choice(
                _genericContactSensorsPort.Subscribe(_genericContactSensorsNotify, typeof(contactsensor.Update)),
                delegate(SubscribeResponseType response) { LogInfo("Subscribed to bumpers"); },
                delegate(Fault f) { LogError(f); }
            );


            yield return Arbiter.Choice(
                _sickLaserRangeFinderPort.Subscribe(_sickLaserRangeFinderNotify, typeof(sicklrf.Replace)),
                delegate(SubscribeResponseType response) { LogInfo("Subscribed to SickLRF"); },
                delegate(Fault f) { LogError(f); }
            );

            yield return Arbiter.Choice(
                _webcam1Port.Subscribe(_webcam1Notify, typeof(webcam.UpdateFrame)),
                delegate(SubscribeResponseType response) { LogInfo("Subscribed to webcam1"); },
                delegate(Fault f) { LogError(f); }
            );

            yield return Arbiter.Choice(
                _webcam2Port.Subscribe(_webcam2Notify, typeof(webcam.UpdateFrame)),
                delegate(SubscribeResponseType response) { LogInfo("Subscribed to webcam2"); },
                delegate(Fault f) { LogError(f); }
            );

            yield return Arbiter.Choice(
               _gpsPort.Subscribe(_gpsNotify, typeof(gps.Replace)),
               delegate(SubscribeResponseType response) { LogInfo("Subscribed to gps"); },
               delegate(Fault f) { LogError(f); }
           );

            MainPortInterleave.CombineWith(
                Arbiter.Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(),
                    new ConcurrentReceiverGroup(
                        Arbiter.ReceiveWithIterator<contactsensor.Update>(true, _genericContactSensorsNotify, GenericContactSensorsUpdateHandler),
                        Arbiter.ReceiveWithIterator<sicklrf.Replace>(true, _sickLaserRangeFinderNotify, SickLaserRangeFinderReplaceHandler),
                        Arbiter.ReceiveWithIterator<webcam.UpdateFrame>(true, _webcam1Notify, WebCam1UpdateFrameHandler),
                        Arbiter.ReceiveWithIterator<webcam.UpdateFrame>(true, _webcam1Notify, WebCam2UpdateFrameHandler),
                        Arbiter.ReceiveWithIterator<gps.Replace> (true, _gpsNotify, GPSReplaceHandler)
                    )
                )
            );

            // Start the car
            yield return Arbiter.Choice(
                _genericDifferentialDrivePort.EnableDrive(true),
                delegate(DefaultUpdateResponseType respone) { },
                delegate(Fault f) { LogError(f); });

            yield return Arbiter.Choice(
                _genericDifferentialDrivePort.SetDrivePower(0.5d, 0.5d),
                delegate(DefaultUpdateResponseType respone) { },
                delegate(Fault f) { LogError(f); });

            yield break;
        }


        #region Standard DSS message handlers

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(_state));
            yield break;
        }

        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReplaceHandler(Replace replace)
        {
            _state = replace.Body;

            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            base.SendNotification<Replace>(_subMgr, replace);

            yield break;
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> SubscribeHandler(Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;

            yield return Arbiter.Choice(
                SubscribeHelper(_subMgr, request, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    base.SendNotificationToTarget<Replace>(request.Subscriber, _subMgr, _state);
                },
                delegate(Exception e) { }
            );
        }

        #endregion


        #region Notification message handlers

        /// <summary>
        /// Receive a contact sensor message
        /// </summary>
        private IEnumerator<ITask> GenericContactSensorsUpdateHandler(contactsensor.Update message)
        {
            // Example
            //if (message.Body.Pressed)
            //{
            //    yield return Arbiter.Choice(
            //    _genericDifferentialDrivePort.AllStop(),
            //    delegate(DefaultUpdateResponseType respone) { },
            //    delegate(Fault f) { LogError(f); });
            //}

            yield break;
        }

        /// <summary>
        /// Receive a sick lrf message
        /// </summary>
        private IEnumerator<ITask> SickLaserRangeFinderReplaceHandler(sicklrf.Replace message)
        {
            // Example
            //int len = message.Body.DistanceMeasurements.Length;
            //int min = 8000;
            //for (int i = (len * 3) / 8; i < (len * 5) / 8; ++i)
            //{
            //    if (message.Body.DistanceMeasurements[i] < min)
            //    {
            //        min = message.Body.DistanceMeasurements[i];
            //    }
            //}
            //if (min < 2000 && min > 200) // Ignore Simulated LRF Error
            //{
            //    yield return Arbiter.Choice(
            //    _genericDifferentialDrivePort.AllStop(),
            //    delegate(DefaultUpdateResponseType respone) { },
            //    delegate(Fault f) { LogError(f); });
            //}

            yield break;
        }

        /// <summary>
        /// Receive WebCam message
        /// </summary>
        private IEnumerator<ITask> WebCam1UpdateFrameHandler(webcam.UpdateFrame message)
        {
            // Example
            webcam.QueryFrameRequest req = new webcam.QueryFrameRequest();
            req.Format = System.Drawing.Imaging.ImageFormat.Bmp.Guid;
            req.Size = new Microsoft.Robotics.PhysicalModel.Proxy.Vector2(128, 128);
            yield return Arbiter.Choice<webcam.QueryFrameResponse, W3C.Soap.Fault>(
                _webcam1Port.QueryFrame(req),
                delegate(webcam.QueryFrameResponse response)
                {

                    try
                    {
                        //System.IO.MemoryStream mem = new System.IO.MemoryStream(response.Frame);
                        //System.Drawing.Bitmap bmp = new System.Drawing.Bitmap(mem);
                        // ANALYZE BITMAP
                    }
                    catch (Exception)
                    {
                    }

                },
                delegate(W3C.Soap.Fault fault)
                {
                });
        }

        /// <summary>
        /// Receive WebCam message
        /// </summary>
        private IEnumerator<ITask> WebCam2UpdateFrameHandler(webcam.UpdateFrame message)
        {
            // Example
            webcam.QueryFrameRequest req = new webcam.QueryFrameRequest();
            req.Format = System.Drawing.Imaging.ImageFormat.Bmp.Guid;
            req.Size = new Microsoft.Robotics.PhysicalModel.Proxy.Vector2(128, 128);
            yield return Arbiter.Choice<webcam.QueryFrameResponse, W3C.Soap.Fault>(
                _webcam2Port.QueryFrame(req),
                delegate(webcam.QueryFrameResponse response)
                {

                    try
                    {
                        //System.IO.MemoryStream mem = new System.IO.MemoryStream(response.Frame);
                        //System.Drawing.Bitmap bmp = new System.Drawing.Bitmap(mem);
                        // ANALYZE BITMAP
                    }
                    catch (Exception)
                    {
                    }

                },
                delegate(W3C.Soap.Fault fault)
                {
                });
        }

        /// <summary>
        /// Receive GPS message
        /// </summary>
        private IEnumerator<ITask> GPSReplaceHandler(gps.Replace message)
        {
            double latitude = message.Body.Position.Latitude;
            double longitude = message.Body.Position.Longitude;

            // Reach end
            if (latitude > 47.60062d && latitude < 47.60065d
                && longitude > 122.32425d && longitude < 122.32428d)
            {
                yield return Arbiter.Choice(
                _genericDifferentialDrivePort.AllStop(),
                delegate(DefaultUpdateResponseType respone) { },
                delegate(Fault f) { LogError(f); });
            }

            yield break;
        }

        #endregion

    }
}
